#include "location.hpp"

// void localodomCallback(const nav_msgs::OdometryConstPtr &msg)
// {
//     current_point.x= msg->pose.pose.position.x;
//     current_point.y= msg->pose.pose.position.y;
//     current_point.z= msg->pose.pose.position.z;

//     current_angle_Q = Eigen::Quaterniond(msg->pose.pose.orientation.w, msg->pose.pose.orientation.x,
//                                     msg->pose.pose.orientation.y, msg->pose.pose.orientation.z);
//     current_angle.x= msg->pose.pose.orientation.x;
//     current_angle.y= msg->pose.pose.orientation.y;
//     current_angle.z= msg->pose.pose.orientation.z;
//     current_angle.w= msg->pose.pose.orientation.w;
//     // curr_angle = toEulerAngle(current_angle);
    
// }

void imuCallback(const sensor_msgs::Imu& msg)
{
    current_angle_Q = Eigen::Quaterniond(msg.orientation.w,msg.orientation.x,msg.orientation.y,msg.orientation.z);
}

void cloud1Callback(const sensor_msgs::PointCloud2ConstPtr& img) {
    //receive cloud message
    pcl::PointCloud<pcl::PointXYZI> latest_cloud1;
    pcl::fromROSMsg(*img, latest_cloud1);
    if (latest_cloud1.points.size() == 0)
    {   
        std::cout << "no cloud points" << std::endl;
        return;
    } 
    // Rpg means Rotation of lidar in ground coord
    // geometry_msgs::Quaternion lidar_angle_q_temp = tf:: createQuaternionMsgFromRollPitchYaw(lidar1_roll,lidar1_pitch,lidar1_yaw);
    // Eigen::Quaterniond lidar_angle_q1(lidar_angle_q_temp.w, lidar_angle_q_temp.x, lidar_angle_q_temp.y, lidar_angle_q_temp.z);
    // current_angle_Q = lidar_angle_q1;

    pcl::PointCloud<pcl::PointXYZI> cloud_transform = transform_pc2map(latest_cloud1, current_angle_Q, lidar1_x, lidar1_y, lidar1_z);
    // pcl::PointCloud<pcl::PointXYZI> cloud_transform = transform_pc2map(latest_cloud1, current_angle_Q, current_point.x, current_point.y, current_point.z);

    // cloud_transform.width = cloud_transform.points.size();
    // cloud_transform.height = 1;
    // cloud_transform.is_dense = true;
    // cloud_transform.header.frame_id = "map";
    // sensor_msgs::PointCloud2 cloud_msg;
    // pcl::toROSMsg(cloud_transform, cloud_msg);
    // map1_pub.publish(cloud_msg);
    //save the last  and last_last cloud 

    // cloud1_last_last_last.swap(cloud1_last_last);
    // cloud1_last_last.swap(cloud1_last);
    cloud1_last.swap(cloud1);
    cloud1.swap(cloud_transform);
}

void select_numCallback(const std_msgs::UInt8 &msg)
{
    select_id = msg.data;
}

void cloud_calc_callback(const ros::TimerEvent& /*event*/) 
{
    // pcl::PointCloud<pcl::PointXYZI> lidar_fusion1 = cloud1_last + cloud1;
    // pcl::PointCloud<pcl::PointXYZI> lidar_fusion2 = lidar_fusion1+cloud1_last_last;
    // pcl::PointCloud<pcl::PointXYZI> lidar_fusion3 = lidar_fusion2+cloud1_last_last_last;

    pcl::PointCloud<pcl::PointXYZI> lidar_fusion3 = cloud1_last+cloud1;


    pcl::PointCloud<pcl::PointXYZI>::Ptr global_cloud(new pcl::PointCloud<pcl::PointXYZI>);
    global_cloud = lidar_fusion3.makeShared();

    lidar_fusion3.width = lidar_fusion3.points.size();
    lidar_fusion3.height = 1;
    lidar_fusion3.is_dense = true;
    lidar_fusion3.header.frame_id = "map";
    sensor_msgs::PointCloud2 cloud_msg;
    pcl::toROSMsg(lidar_fusion3, cloud_msg);
    map1_pub.publish(cloud_msg);

    Eigen::Matrix3d Rpg_temp1 = current_angle_Q.toRotationMatrix();
    //current R x vector
    geometry_msgs::Vector3 x_vector1;
    x_vector1.x = Rpg_temp1(0,0);
    x_vector1.y = Rpg_temp1(1,0);
    x_vector1.z = Rpg_temp1(2,0);
    
    visualization_msgs::Marker line_marker = draw_line_marker(x_vector1, 100);
    marker2_pub.publish(line_marker);
    bool selected_flag = false;
    if(global_cloud->points.size()>0)
    {
        // ROS_INFO("here calculate point cloud obstacle...");
        pcl::search::KdTree<pcl::PointXYZI>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZI>);
        tree->setInputCloud(global_cloud);
        std::vector<pcl::PointCloud<pcl::PointXYZI>> clusters;//the cluster finnaly
        
        std::vector<pcl::PointIndices> clusterIndices;// create indices class
        pcl::EuclideanClusterExtraction<pcl::PointXYZI> ec; //euclidean cluster class
        ec.setClusterTolerance(clusterTolerance);
        ec.setMinClusterSize(minsize);
        ec.setMaxClusterSize(maxsize);
        ec.setSearchMethod(tree);//use kd tree algorithm fo searching
        ec.setInputCloud(global_cloud); // feed point cloud
        ec.extract(clusterIndices); 
        
        for (pcl::PointIndices getIndices : clusterIndices)
        {
            pcl::PointCloud<pcl::PointXYZI> cloudCluster;
            // For each point indice in each cluster
            for (int index : getIndices.indices) 
            {
                cloudCluster.push_back(global_cloud->points[index]);
            }
            cloudCluster.width = cloudCluster.size();
            cloudCluster.height = 1;
            cloudCluster.is_dense = true;
            clusters.push_back(cloudCluster);
        }
        if(clusters.size()==0)
        {
            return;
        }

        int clusterId = 0;
        double min_dist_angle = 99999;
        double max_height = -1.0;
        geometry_msgs::Vector3 min_dist_vect;
        geometry_msgs::Vector3 max_height_vect;
        for (pcl::PointCloud<pcl::PointXYZI> cluster : clusters)
        {
            pcl::PointXYZI minPoint, maxPoint;
            pcl::getMinMax3D(cluster, minPoint, maxPoint); //get x,y,z min and max
            Box box_temp;
            box_temp.x_min = minPoint.x-(inflation);
            box_temp.y_min = minPoint.y-(inflation);
            box_temp.z_min = minPoint.z-inflation;
            box_temp.x_max = maxPoint.x+(inflation);
            box_temp.y_max = maxPoint.y+(inflation);
            box_temp.z_max = maxPoint.z+inflation;

            float box_center_x = (box_temp.x_min+box_temp.x_max)/2;
            float box_center_y = (box_temp.y_min+box_temp.y_max)/2;
            float box_center_z = (box_temp.z_min+box_temp.z_max)/2;


            visualization_msgs::Marker cluster_marker = draw_cluster_marker(clusterId, box_temp);
            marker_pub.publish(cluster_marker);
            
            

            //track nearest cluster from ray_line
            // double distance_by_angle = abs(x_vector1.x*box_center_x+x_vector1.y*box_center_y+x_vector1.z*box_center_z);
            // if(min_dist_angle>distance_by_angle)
            // {
            //     min_dist_angle=distance_by_angle;
            //     min_dist_vect.x = box_center_x;
            //     min_dist_vect.y = box_center_y;
            //     min_dist_vect.z = box_center_z;
            //     selected_flag=true;
            // }

            //track highest cluster from ground
            if(max_height<box_center_z)
            {
                max_height = box_center_z;
                max_height_vect.x = box_center_x;
                max_height_vect.y = box_center_y;
                max_height_vect.z = box_center_z;
                selected_flag=true;
            }

            

            clusterId++;
        } 
        float selected_x = max_height_vect.x;
        float selected_y = max_height_vect.y;
        float selected_z = max_height_vect.z;

        
        //judge condition:
        if(selected_flag) //&& id_list[select_id].existed
        {
            nav_msgs::Odometry plane_position;
            plane_position.header.stamp = ros::Time::now();
            plane_position.header.frame_id = "map";
            // plane_position.child_frame_id = "odom";
            

            plane_position.pose.pose.position.x = selected_x;
            plane_position.pose.pose.position.y = selected_y;
            plane_position.pose.pose.position.z = selected_z;
            //send target pos to p900
            std::cout<<"---->plane_position: "<<selected_x<<" , "<<selected_y<<" , "<<selected_z<<std::endl;
            plane_pos_pub.publish(plane_position);

            //calculate the offset to center ray
            //test 
            current_point.x = 0;
            current_point.y = 0;
            current_point.z = 0;
            
            Eigen::Matrix3d Rpg_temp = current_angle_Q.toRotationMatrix();
            //current R x vector
            geometry_msgs::Vector3 x_vector;
            x_vector.x = Rpg_temp(0,0);
            x_vector.y = Rpg_temp(1,0);
            x_vector.z = Rpg_temp(2,0);

            // std::cout<<"---->x_vector: "<<x_vector.x<<" , "<<x_vector.y<<" , "<<x_vector.z<<std::endl;
            
            //dist vector need to go
            geometry_msgs::Vector3 x_dist_vector;
            x_dist_vector.x = selected_x-current_point.x;
            x_dist_vector.y = selected_y-current_point.y;
            x_dist_vector.z = selected_z-current_point.z;
            // std::cout<<"---->x_dist_vector: "<<x_dist_vector.x<<" , "<<x_dist_vector.y<<" , "<<x_dist_vector.z<<std::endl;
            

            float x_vector_yaw = atan2(x_vector.y,x_vector.x);
            float x_vector_pitch = atan2(x_vector.z,sqrt(x_vector.x*x_vector.x+x_vector.y*x_vector.y));

            float x_dist_vector_yaw = atan2(x_dist_vector.y,x_dist_vector.x);
            //static yaw 
            // float x_dist_vector_yaw = -1.571;

            float x_dist_vector_pitch = atan2(x_dist_vector.z,sqrt(x_dist_vector.x*x_dist_vector.x+x_dist_vector.y*x_dist_vector.y));
            // std::cout<<"---->angle error: "<<x_dist_vector_pitch*180/3.141593<<" , "<<x_vector_pitch*180/3.141593<<std::endl;

            float yaw_error = x_dist_vector_yaw-x_vector_yaw;
            float pitch_error = x_dist_vector_pitch-x_vector_pitch;
            

            //calculate angle rate with p control
            float yaw_rate = angle_rate_p_control(yaw_error*180/3.141593, 5,0,90);
            float pitch_rate = angle_rate_p_control(pitch_error*180/3.141593, 10,0,90);

            std_msgs::Float32MultiArray angle_rate2pub;
            angle_rate2pub.data.push_back(yaw_rate);
            

            angle_rate2pub.data.push_back(pitch_rate);
            duoji_rate_pub.publish(angle_rate2pub);
            

        } 
        else{
            std_msgs::Float32MultiArray angle_rate2pub;
            angle_rate2pub.data.push_back(0.0);
            angle_rate2pub.data.push_back(0.0);
            duoji_rate_pub.publish(angle_rate2pub);

        }  
    }
    
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "ground_location");
    ros::NodeHandle nh;

    //read parameters from launch file
    nh.param<bool>("simulation",simulation,false);
    nh.param<double>("inflation",inflation,5.0);

    nh.param<double>("map_size_x",map_size_x,100.0);
    nh.param<double>("map_size_y",map_size_y,100.0);
    nh.param<double>("map_size_z_min",map_size_z_min,5.0);
    nh.param<double>("map_size_z_max",map_size_z_max,15.0);
    
    nh.param<double>("sample_size",sample_size,0.5);
    
    nh.param<double>("clusterTolerance",clusterTolerance,1.0);
    nh.param<int>("minsize",minsize,5);
    nh.param<int>("maxsize",maxsize,1000);

    nh.param<double>("min_lidar_distance",min_lidar_distance,2.0);
    nh.param<double>("max_lidar_distance",max_lidar_distance,100.0);
    nh.param<double>("lidar_intensity",lidar_intensity,50.0);
    nh.param<double>("track_search_field",track_search_field,1.0);
    

    nh.param<double>("lidar1_roll",lidar1_roll,0.0);
    nh.param<double>("lidar1_pitch",lidar1_pitch,0.0);
    nh.param<double>("lidar1_yaw",lidar1_yaw,0.0);
    nh.param<double>("lidar1_x",lidar1_x,0.0);
    nh.param<double>("lidar1_y",lidar1_y,0.0);
    nh.param<double>("lidar1_z",lidar1_z,0.0);


    cloud_pub_timer = nh.createTimer(ros::Duration(0.1), cloud_calc_callback);
    if(simulation)
    {
        lidar1_cloud_sub = nh.subscribe("/dijileida/realsense/depth_camera/depth/points",1,cloud1Callback);

    }
    else
    {
        lidar1_cloud_sub = nh.subscribe("/livox/lidar",1,cloud1Callback);
    }

    // local_position_sub = nh.subscribe("/mavros/local_position/odom",1,localodomCallback);
    imu_sub = nh.subscribe("/mavros0/imu/data",1,imuCallback);
    // imu_sub = nh.subscribe("/uav0/mavros/imu/data",1,imuCallback);
    select_num_sub = nh.subscribe("/rviz/select_number_id",1,select_numCallback);
    

    marker_pub = nh.advertise<visualization_msgs::Marker>("/kdtree_box", 1);
    marker2_pub = nh.advertise<visualization_msgs::Marker>("/ray_line", 1);
    marker_id_pub = nh.advertise<visualization_msgs::Marker>("/drone_id_marker", 1);
    map1_pub = nh.advertise<sensor_msgs::PointCloud2>("/pcl/cloud1", 1);


    plane_pos_pub = nh.advertise<nav_msgs::Odometry>("/pcl/odom", 1);

    duoji_rate_pub = nh.advertise<std_msgs::Float32MultiArray>("/destination_servo_angle",1);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(100);    
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
 
    return 0;
}


pcl::PointCloud<pcl::PointXYZI> transform_pc2map(pcl::PointCloud<pcl::PointXYZI> latest_cloud, Eigen::Quaterniond lidar_angle_q,
                    double lidar_x, double lidar_y, double lidar_z)
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZI>);
    cloud_in = latest_cloud.makeShared();
    //sample to voxel grid 
    pcl::PCLPointCloud2::Ptr cloud_in2 (new pcl::PCLPointCloud2 ());
	pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
	toPCLPointCloud2(*cloud_in,*cloud_in2);
	pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
	sor.setInputCloud (cloud_in2);
	sor.setLeafSize(sample_size,sample_size,sample_size);
	sor.filter (*cloud_filtered);
	fromPCLPointCloud2(*cloud_filtered,*cloud_in);
    
    pcl::PointXYZI pt;
    // pitch 90
    Eigen::Matrix3d Rpitch;
    Rpitch<< 0, 0, 1,
        0, 1, 0,
        -1, 0, 0;
    // roll 90
    Eigen::Matrix3d Rroll;
    Rroll<< 1, 0, 0,
            0, 0, 1,
            0, -1, 0;

    // yaw 45
    Eigen::Matrix3d Ryaw;
    Ryaw<< 0.707, -0.707, 0,
            0.707, 0.707, 0,
            0, 0, 1;
    
    Eigen::Matrix3d Rpg = lidar_angle_q.toRotationMatrix();


    pcl::PointCloud<pcl::PointXYZI> cloud_out;
    
    for (size_t i = 0; i < cloud_in->points.size(); ++i) 
    {
        if (!pcl::isFinite(cloud_in->points[i]))
        {
            continue;
        }
        //delete points too near to lidar ,such as itself
        double dist_body = sqrt(cloud_in->points[i].x*cloud_in->points[i].x+cloud_in->points[i].y*cloud_in->points[i].y+cloud_in->points[i].z*cloud_in->points[i].z);
        if(abs(dist_body)<min_lidar_distance || abs(dist_body)>max_lidar_distance)
        {
            continue;
        }
        if(!simulation)
        {
            if(cloud_in->points[i].intensity<lidar_intensity)
            {
                continue;
            }
        }

        Eigen::Vector3d PointE(cloud_in->points[i].x,cloud_in->points[i].y,cloud_in->points[i].z);
        //rotate pitch and roll 90 degree because the coordinate differences if in sumilation
        if(simulation)
        {
            PointE =  Rpitch*PointE;
            PointE =  Rroll*PointE;
        }
        
        //rotate the point cloud to the vehicle coordinate
        //by zhou simulation
        if(simulation)
        {
            PointE =  Ryaw*PointE;
            pt.x = PointE(0) -5.0;
            pt.y = PointE(1) -5.0;
            pt.z = PointE(2) + 1.0;
        }
        else
        {
            PointE =  Rpg*PointE;
            pt.x = PointE(0) + lidar_x;
            pt.y = PointE(1) + lidar_y;
            pt.z = PointE(2) + lidar_z;
        }
        

        //final_point = Rotation*origin_point + Transform
        
        //judge what type of points push_back to cloud:temp x,y~(trajectory) , z~(3,10)
        if(pt.z>map_size_z_min && pt.z<map_size_z_max && abs(pt.x)<map_size_x && abs(pt.y)<map_size_y) 
        {
            cloud_out.push_back(pt);    
        }  
    }
    return cloud_out;
}



visualization_msgs::Marker draw_line_marker(geometry_msgs::Vector3 vector, int length)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "vector_line";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = 1; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    // line_strip.lifetime = ros::Duration(0.1);
    line_strip.scale.x = 0.05; //width of the line
    line_strip.color.r = 0.1; line_strip.color.g = 0.2; line_strip.color.b = 0.5;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[2];
    p[0].x = 0;     p[0].y = 0;     p[0].z = 0;
    p[1].x = length*vector.x;     p[1].y = length*vector.y;     p[1].z = length*vector.z;


    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[1]);

    
    return line_strip;
    
    //end draw
}

visualization_msgs::Marker draw_cluster_marker(int clusterId, Box box)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = clusterId; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(0.5);
    line_strip.scale.x = 0.1; //width of the line
    line_strip.color.r = 0.0; line_strip.color.g = 0.5; line_strip.color.b = 0.5;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = box.x_min;     p[0].y = box.y_max;     p[0].z = box.z_max;
    p[1].x = box.x_min;     p[1].y = box.y_min;     p[1].z = box.z_max;
    p[2].x = box.x_min;     p[2].y = box.y_min;     p[2].z = box.z_min;
    p[3].x = box.x_min;     p[3].y = box.y_max;     p[3].z = box.z_min;
    p[4].x = box.x_max;     p[4].y = box.y_max;     p[4].z = box.z_min;
    p[5].x = box.x_max;     p[5].y = box.y_min;     p[5].z = box.z_min;
    p[6].x = box.x_max;     p[6].y = box.y_min;     p[6].z = box.z_max;
    p[7].x = box.x_max;     p[7].y = box.y_max;     p[7].z = box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    
    return line_strip;
    
    //end draw
}

visualization_msgs::Marker draw_cluster_id_marker(Point_with_id id_point) 
{
    visualization_msgs::Marker marker;
    marker.header.frame_id="map";
    marker.header.stamp = ros::Time::now();
    marker.ns = "basic_shapes";
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.id =id_point.id;
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    // marker.lifetime = ros::Duration(0.1);

    if(id_point.id==select_id)
    {
        marker.scale.z = 0.3;
        marker.color.b = 0;
        marker.color.g = 255;
        marker.color.r = 255;
        marker.color.a = 1;
    }
    else{
        marker.scale.z = 0.1;
        marker.color.b = 0;
        marker.color.g = 0;
        marker.color.r = 255;
        marker.color.a = 1;
    }
    
    

    geometry_msgs::Pose pose_name;
    pose_name.position.x =  id_point.x+0.3;
    pose_name.position.y =  id_point.y+0.3;
    pose_name.position.z =  id_point.z+0.3;
    std::ostringstream str;
    str<<id_point.id;
    marker.text=str.str();
    marker.pose=pose_name;

    return marker;

}

float angle_rate_p_control(float angle_error, float P, float dead_zone, float angle_rate_limit)
{
    float out_put_rate=0.0;
    if (angle_error < -180)
        angle_error += 360;
    else if (angle_error > 180)
        angle_error -= 360;
    out_put_rate = P * angle_error;

    if (abs(angle_error) <= dead_zone) {
        out_put_rate = 0.0;
    }
    if (abs(out_put_rate) >= angle_rate_limit) {
        out_put_rate = limit(out_put_rate, angle_rate_limit);
    } 
    return out_put_rate;
}
        